package scu.maqiang.optimalcontrol;

import scu.maqiang.numeric.MVO;

import java.util.Arrays;

public class OptimalControl {
    /**
     * 用于在最优控制中线搜索Armijo回溯法
     * @param func 目标泛函表达式
     * @param c2sOperator 控制-状态算子函数
     * @param decentDirection 下降方向向量
     * @param decentDotNGrad 下降方向与负梯度方向的内积
     * @param beta 下降比例
     * @param sigma Armijo控制参数
     * @param state 状态向量
     * @param control 控制向量
     * @return 控制步长
     */
    public static double ArmijoStep(CostFunctional func, Operator c2sOperator, double[] decentDirection,
                                    double decentDotNGrad, double beta, double sigma, double[] state, double[] control) {
        int m = 0;
        int mmax = 20;
        double alpha = 1.0;
        double[] newControl = new double[control.length];
        double[] newState = new double[state.length];
        double cost = func.apply(state, control);
        while (m <= mmax) {
            //newControl = control + alpha * decentDirection
            MVO.add(control, alpha, decentDirection, newControl);
            c2sOperator.operate(newControl, newState);
            if (func.apply(newState, newControl) < cost - sigma * alpha * decentDotNGrad) {
                break;
            }
            alpha = alpha * beta;
            m = m + 1;
        }
        return alpha;
    }

    public void Projection(double[] u, double u_a, double u_b) {
        Arrays.setAll(u, i -> Math.max(u_a, Math.min(u[i], u_b)));
    }
}
